#include <iostream>
#include "../inc/HoughSpace.h"

using namespace std;

HoughSpace::HoughSpace()
{
	bot.setXYZ(0,0,0);
	top.setXYZ(0,0,0);
	
	nbFrameA = 0;
	nbFrameB = 0;
	nbFrameR = 0;
	
	aFrameMax = 0;
	bFrameMax = 0;
	rFrameMax = 0;

	frameSizeA = 0;
	frameSizeB = 0;
	frameSizeR = 0;
	
	scoreMax = 0;
	scoreTotal = 0;
	
	frames = NULL;
}

HoughSpace::HoughSpace(int _mode, Point _bot, Point _top, float _a, float _b, float _r)
{
	bot = _bot;
	top = _top;
	
	aFrameMax = 0;
	bFrameMax = 0;
	rFrameMax = 0;
	
	scoreMax = 0;
	scoreTotal = 0;
	
	if ( _mode == RESOLUTION )
	{
		nbFrameA = (int)_a;
		nbFrameB = (int)_b;
		nbFrameR = (int)_r;
		
		frameSizeA = ( _top.getX() - _bot.getX() ) / nbFrameA;
		frameSizeB = ( _top.getY() - _bot.getY() ) / nbFrameB;
		frameSizeR = ( _top.getZ() - _bot.getZ() ) / nbFrameR;
	}
	
	if ( _mode == SIZE )
	{
		frameSizeA = _a;
		frameSizeB = _b;
		frameSizeR = _r;
		
		nbFrameA = (int) (( _top.getX() - _bot.getX() ) / frameSizeA) + 1;
		nbFrameB = (int) (( _top.getY() - _bot.getY() ) / frameSizeB) + 1;
		nbFrameR = (int) (( _top.getZ() - _bot.getZ() ) / frameSizeR) + 1;
	}
	
	if ( _mode != SIZE && _mode != RESOLUTION )
	{
		cout<<"Mauvais parametre pour la création de HoughSpace : seuls RESOLUTION et SIZE sont acceptés comme mode"<<endl;
	}
	
	frames = (HoughFrame***) malloc ( nbFrameA * sizeof ( HoughFrame** ) );
	for ( int i = 0 ; i < nbFrameA ; i++ )
	{
		frames[i] = (HoughFrame**) malloc ( nbFrameB * sizeof ( HoughFrame* ) );
		
		for ( int j = 0 ; j < nbFrameB ; j++ )
		{
			frames[i][j] = (HoughFrame*) malloc ( nbFrameR * sizeof (HoughFrame) );
			
			for ( int k = 0 ; k < nbFrameR ; k++ )
			{
				Point pBot, pTop;
				
				pBot.setXYZ( bot.getX() + ( i * frameSizeA), bot.getY() + ( j * frameSizeB ), bot.getZ() + ( k * frameSizeR ));
				pTop.setXYZ( pBot.getX() + frameSizeA, pBot.getY() + frameSizeB, pBot.getZ() + frameSizeR );
				
				frames[i][j][k].setPointBot( pBot );
				frames[i][j][k].setPointTop( pTop );
				
				frames[i][j][k].setScore( 0 );
			}
		}
	}
}

HoughSpace::~HoughSpace()
{
	for ( int i = 0 ; i < nbFrameA ; i++ )
	{
		for ( int j = 0 ; j < nbFrameB ; j++ )
		{
			free(frames[i][j]);
		}
		
		free (frames[i]);
	}
	
	free(frames);
}

void HoughSpace::init(int _mode, Point _bot, Point _top, float _a, float _b, float _r)
{
	if ( frames != NULL )
	{
		cout<<"Erreur, l'espace de Hough a déjà été initialisé"<<endl;
		return;
	}

	bot = _bot;
	top = _top;
	
	aFrameMax = 0;
	bFrameMax = 0;
	rFrameMax = 0;
	
	scoreMax = 0;
	scoreTotal = 0;
	
	if ( _mode == RESOLUTION )
	{
		nbFrameA = (int)_a;
		nbFrameB = (int)_b;
		nbFrameR = (int)_r;
		
		frameSizeA = ( _top.getX() - _bot.getX() ) / nbFrameA;
		frameSizeB = ( _top.getY() - _bot.getY() ) / nbFrameB;
		frameSizeR = ( _top.getZ() - _bot.getZ() ) / nbFrameR;
	}
	
	if ( _mode == SIZE )
	{
		frameSizeA = _a;
		frameSizeB = _b;
		frameSizeR = _r;
		
		nbFrameA = (int) (( _top.getX() - _bot.getX() ) / frameSizeA) + 1;
		nbFrameB = (int) (( _top.getY() - _bot.getY() ) / frameSizeB) + 1;
		nbFrameR = (int) (( _top.getZ() - _bot.getZ() ) / frameSizeR) + 1;
	}
	
	if ( _mode != SIZE && _mode != RESOLUTION )
	{
		cout<<"Mauvais parametre pour la création de HoughSpace : seuls RESOLUTION et SIZE sont acceptés comme mode"<<endl;
	}
	
	frames = (HoughFrame***) malloc ( nbFrameA * sizeof ( HoughFrame** ) );
	for ( int i = 0 ; i < nbFrameA ; i++ )
	{
		frames[i] = (HoughFrame**) malloc ( nbFrameB * sizeof ( HoughFrame* ) );
		
		for ( int j = 0 ; j < nbFrameB ; j++ )
		{
			frames[i][j] = (HoughFrame*) malloc ( nbFrameR * sizeof (HoughFrame) );
			
			for ( int k = 0 ; k < nbFrameR ; k++ )
			{
				Point pBot, pTop;
				
				pBot.setXYZ( bot.getX() + ( i * frameSizeA), bot.getY() + ( j * frameSizeB ), bot.getZ() + ( k * frameSizeR ));
				pTop.setXYZ( pBot.getX() + frameSizeA, pBot.getY() + frameSizeB, pBot.getZ() + frameSizeR );
				
				frames[i][j][k].setPointBot( pBot );
				frames[i][j][k].setPointTop( pTop );
				
				frames[i][j][k].setScore( 0 );
			}
		}
	}
}

int HoughSpace::getNbFrameA()
{
	return this->nbFrameA;
}

int HoughSpace::getNbFrameB()
{
	return this->nbFrameB;
}

int HoughSpace::getNbFrameR()
{
	return this->nbFrameR;
}

float HoughSpace::getFrameSizeA()
{
	return this->frameSizeA;
}

float HoughSpace::getFrameSizeB()
{
	return this->frameSizeB;
}

float HoughSpace::getFrameSizeR()
{
	return this->frameSizeR;
}

float HoughSpace::getAFrameMax()
{
	return this->aFrameMax;
}

float HoughSpace::getBFrameMax()
{
	return this->bFrameMax;
}

float HoughSpace::getRFrameMax()
{
	return this->rFrameMax;
}

Point HoughSpace::getBot()
{
	return this->bot;
}
	
Point HoughSpace::getTop()
{
	return this->top;
}


int HoughSpace::getScoreMax()
{
	return this->scoreMax;
}

int HoughSpace::getScore(int _a, int _b, int _r)
{
	return this->frames[_a][_b][_r].getScore();
}

int HoughSpace::getScoreTotal()
{
	return this->scoreTotal;
}

HoughFrame HoughSpace::getHoughFrame(int _a, int _b, int _r)
{
	return this->frames[_a][_b][_r];
}

void HoughSpace::setAFrameMax(float _aFrameMax)
{
	this->aFrameMax = _aFrameMax;
}

void HoughSpace::setBFrameMax(float _bFrameMax)
{
	this->bFrameMax = _bFrameMax;
}

void HoughSpace::setRFrameMax(float _rFrameMax)
{
	this->rFrameMax = _rFrameMax;
}
	
void HoughSpace::setScoreMax(int _scoreMax)
{
	this->scoreMax = _scoreMax;
}

void HoughSpace::fill(vector <Circle> _tabCircles)
{
	int a, b, r;
	scoreMax = 0;

	if(_tabCircles.size() > 0)
	{
		for(int i=0; i < (int)_tabCircles.size(); i++)
		{
			_tabCircles[i].setABR(_tabCircles[i].getA() - bot.getX(), _tabCircles[i].getB() - bot.getY(), _tabCircles[i].getR() - bot.getZ());

			a = (int)((_tabCircles[i].getA())/frameSizeA);
			b = (int)((_tabCircles[i].getB())/frameSizeB);
			r = (int)((_tabCircles[i].getR())/frameSizeR);

			if ( a >= nbFrameA )
				a = nbFrameA-1;
			
			if ( b >= nbFrameB )
				b = nbFrameB-1;
			
			if ( r >= nbFrameR )
				r = nbFrameR-1;
			
			_tabCircles[i].setABR(_tabCircles[i].getA() + bot.getX(), _tabCircles[i].getB() + bot.getY(), _tabCircles[i].getR() + bot.getZ());

			frames[a][b][r].incrementScore();

			if(frames[a][b][r].getScore() > scoreMax)
			{
				scoreMax = frames[a][b][r].getScore();
				aFrameMax = frames[a][b][r].getBotPoint().getX() + frameSizeA/2;
				bFrameMax = frames[a][b][r].getBotPoint().getY() + frameSizeB/2;
				rFrameMax = frames[a][b][r].getBotPoint().getZ() + frameSizeR/2;
			}
			
		}
	}
}

void HoughSpace::threshold(vector <Circle> &_rslt, int _thresh)
{	
	for ( int i = 0 ; i < nbFrameA ; i++ )
		for ( int j = 0 ; j < nbFrameB ; j++ )
			for ( int k = 0 ; k < nbFrameR ; k++ )
				if ( this->getScore(i,j,k) > _thresh )
				{
					_rslt.push_back ( Circle (  ((float)i * frameSizeA) + this->bot.getX() + (frameSizeA/(float)2),
											   ((float)j * frameSizeB) + this->bot.getY() + (frameSizeB/(float)2),
											   ((float)k * frameSizeR) + this->bot.getZ() + (frameSizeR/(float)2)
											 ));
				}
}

vector<Circle> HoughSpace::threshold(int _thresh)
{
	vector<Circle> rslt;
	
	rslt.clear();
	
	for ( int i = 0 ; i < nbFrameA ; i++ )
		for ( int j = 0 ; j < nbFrameB ; j++ )
			for ( int k = 0 ; k < nbFrameR ; k++ )
			{
				if ( this->getScore(i,j,k) > _thresh )
				{
					rslt.push_back ( Circle (  ((float)i * frameSizeA) + this->bot.getX() + (frameSizeA/(float)2),
											   ((float)j * frameSizeB) + this->bot.getY() + (frameSizeB/(float)2),
											   ((float)k * frameSizeR) + this->bot.getZ() + (frameSizeR/(float)2)
											 ));
				}
			}

	return rslt;
}

vector<Circle> HoughSpace::threshold(float _thresh1, float _thresh2)
{
	vector<Circle> rslt;
	int thresh;
	
	rslt.clear();
	
	for ( int i = 0 ; i < nbFrameA ; i++ )
		for ( int j = 0 ; j < nbFrameB ; j++ )
			for ( int k = 0 ; k < nbFrameR ; k++ )
			{
				thresh = (int)((((1.0/(float)scoreTotal)* (_thresh2 - _thresh1)) + _thresh1) / 100.0) * scoreTotal;

				if ( this->getScore(i,j,k) > thresh )
				{
					rslt.push_back ( Circle (  ((float)i * frameSizeA) + this->bot.getX() + (frameSizeA/(float)2),
											   ((float)j * frameSizeB) + this->bot.getY() + (frameSizeB/(float)2),
											   ((float)k * frameSizeR) + this->bot.getZ() + (frameSizeR/(float)2)
											 ));
				}
			}

	return rslt;
}

void HoughSpace::increment(int _a, int _b, int _r)
{
	this->frames[_a][_b][_r].incrementScore();
	this->scoreTotal ++;
}

void HoughSpace::increment ( int _a, int _b, int _r , int _val)
{
	this->frames[_a][_b][_r].incrementScore(_val);
	this->scoreTotal += _val;
}
